/** @file m_vector.cpp
	@brief 2D, 3D Vectors of several types */

#include "math/m_vector.h"

using namespace G3;

//==============================================================================
// Constructors
//==============================================================================
template <class T>
G3::VEC2<T>::VEC2 (T X, T Y) {
    x = X;
    y = Y;
}

template <class T>
G3::VEC3<T>::VEC3 (T X, T Y, T Z) {
    x = X;
    y = Y;
	z = Z;
}

//==============================================================================
// [] operators
//==============================================================================
template <class T>
T& G3::VEC2<T>::operator[] (const int &i) {
    return (i == 0 ? x : y);
}

template <class T>
T& G3::VEC3<T>::operator[] (const int &i) {
	switch (i) {
		case 0: return x;
		case 1: return y;
	}
    return z;
}

//==============================================================================
// = operators
//==============================================================================
template <class T>
VEC2<T>& G3::VEC2<T>::operator= (const T &t) {
    x = t;
    y = t;
    return *this;
}

template <class T>
VEC2<T>& G3::VEC2<T>::operator= (const VEC2<T> &v) {
    x = v.x;
    y = v.y;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator= (const T &t) {
    x = t;
    y = t;
	z = t;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator= (const VEC3<T> &v) {
    x = v.x;
    y = v.y;
	z = v.z;
    return *this;
}

//==============================================================================
// == operators
//==============================================================================
template <class T>
bool G3::VEC2<T>::operator== (const VEC2<T> &v) const {
    return (x < v.x + G3_EPSILON && x > v.x - G3_EPSILON &&
            y < v.y + G3_EPSILON && y > v.y - G3_EPSILON);
}

template <class T>
bool G3::VEC3<T>::operator== (const VEC3<T> &v) const {
    return (x < v.x + G3_EPSILON && x > v.x - G3_EPSILON &&
            y < v.y + G3_EPSILON && y > v.y - G3_EPSILON &&
            z < v.z + G3_EPSILON && z > v.z - G3_EPSILON);
}

//==============================================================================
// != operators
//==============================================================================
template <class T>
bool G3::VEC2<T>::operator!= (const VEC2<T> &v) const {
    return !(*this == v);
}

template <class T>
bool G3::VEC3<T>::operator!= (const VEC3<T> &v) const {
    return !(*this == v);
}

//==============================================================================
// < operators
//==============================================================================
template <class T>
bool G3::VEC2<T>::operator< (const VEC2<T> &v) const {
    return (x < v.x + G3_EPSILON && y < v.y + G3_EPSILON);
}

template <class T>
bool G3::VEC3<T>::operator< (const VEC3<T> &v) const {
    return (x < v.x + G3_EPSILON && y < v.y + G3_EPSILON &&
            z < v.z + G3_EPSILON);
}

//==============================================================================
// + operators
//==============================================================================
template <class T>
VEC2<T> G3::VEC2<T>::operator+ (const T &t) const {
    return VEC2<T> (x + t, y + t);
}

template <class T>
VEC2<T> G3::VEC2<T>::operator+ (const VEC2<T> &v) const {
    return VEC2<T> (x + v.x, y + v.y);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator+ (const T &t) const {
    return VEC3<T> (x + t, y + t, z + t);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator+ (const VEC3<T> &v) const {
    return VEC3<T> (x + v.x, y + v.y, z + v.z);
}

//==============================================================================
// A-B operators
//==============================================================================
template <class T>
VEC2<T> G3::VEC2<T>::operator- (const T &t) const {
    return VEC2<T> (x - t, y - t);
}

template <class T>
VEC2<T> G3::VEC2<T>::operator- (const VEC2<T> &v) const {
    return VEC2<T> (x - v.x, y - v.y);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator- (const T &t) const {
    return VEC3<T> (x - t, y - t, z - t);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator- (const VEC3<T> &v) const {
    return VEC3<T> (x - v.x, y - v.y, z - v.z);
}

//==============================================================================
// -A operators
//==============================================================================
template <class T>
VEC2<T> G3::VEC2<T>::operator- (void) const {
    return VEC2<T> (-x, -y);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator- (void) const {
    return VEC3<T> (-x, -y, -z);
}

//==============================================================================
// += operators
//==============================================================================
template <class T>
VEC2<T>& G3::VEC2<T>::operator+= (const T &t) {
    x += t;
    y += t;
    return *this;
}

template <class T>
VEC2<T>& G3::VEC2<T>::operator+= (const VEC2<T> &v) {
    x += v.x;
    y += v.y;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator+= (const T &t) {
    x += t;
    y += t;
	z += t;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator+= (const VEC3<T> &v) {
    x += v.x;
    y += v.y;
	z += v.z;
    return *this;
}

//==============================================================================
// -= operators
//==============================================================================
template <class T>
VEC2<T>& G3::VEC2<T>::operator-= (const T &t) {
    x -= t;
    y -= t;
    return *this;
}

template <class T>
VEC2<T>& G3::VEC2<T>::operator-= (const VEC2<T> &v) {
    x -= v.x;
    y -= v.y;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator-= (const T &t) {
    x -= t;
    y -= t;
	z -= t;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator-= (const VEC3<T> &v) {
    x -= v.x;
    y -= v.y;
	z -= v.z;
    return *this;
}

//==============================================================================
// * operators
//==============================================================================
template <class T>
VEC2<T> G3::VEC2<T>::operator* (const T &t) const {
    return VEC2<T> (x * t, y * t);
}

template <class T>
T G3::VEC2<T>::operator* (const VEC2<T> &v) const {
    return (x * v.x + y * v.y);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator* (const T &t) const {
    return VEC3<T> (x * t, y * t, z * t);
}

template <class T>
T G3::VEC3<T>::operator* (const VEC3<T> &v) const {
    return (x * v.x + y * v.y + z * v.z);
}

//==============================================================================
// / operators
//==============================================================================
template <class T>
VEC2<T> G3::VEC2<T>::operator/ (const T &t) const {
    return VEC2<T> (x / t, y / t);
}

template <class T>
VEC3<T> G3::VEC3<T>::operator/ (const T &t) const {
    return VEC3<T> (x / t, y / t, z / t);
}

//==============================================================================
// *= operators
//==============================================================================
template <class T>
VEC2<T>& G3::VEC2<T>::operator*= (const T &t) {
    x *= t;
    y *= t;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator*= (const T &t) {
    x *= t;
    y *= t;
	z *= t;
    return *this;
}

//==============================================================================
// /= operators
//==============================================================================
template <class T>
VEC2<T>& G3::VEC2<T>::operator/= (const T &t) {
    x /= t;
    y /= t;
    return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::operator/= (const T &t) {
    x /= t;
    y /= t;
	z /= t;
    return *this;
}

//==============================================================================
// ::Reset ()
//==============================================================================
template <class T>
VEC2<T>& G3::VEC2<T>::Reset () {
	x = 0;
	y = 0;

	return *this;
}

template <class T>
VEC3<T>& G3::VEC3<T>::Reset () {
	x = 0;
	y = 0;
	z = 0;

	return *this;
}

//==============================================================================
// ::SetCoord ()
//==============================================================================
template <class T>
void G3::VEC2<T>::SetCoord (const int &id, T val) {
	if (id == 0)
		x = val;
	else
		y = val;
}

template <class T>
void G3::VEC3<T>::SetCoord (const int &id, T val) {
	switch (id) {
		case 0: x = val; break;
		case 1: y = val; break;
		default: z = val; break;
	}
}

//==============================================================================
// ::GetCoord ()
//==============================================================================
template <class T>
T G3::VEC2<T>::GetCoord (const int &id) const {
    return (id == 0 ? x : y);
}

template <class T>
T G3::VEC3<T>::GetCoord (const int &id) const {
	switch (id) {
		case 0: return x;
		case 1: return y;
		default: return z;
	}
}

//==============================================================================
// ::GetLength ()
//==============================================================================
template <class T>
double G3::VEC2<T>::GetLength () const {
    return sqrt (x*x + y*y);
}

template <class T>
double G3::VEC3<T>::GetLength () const {
    return sqrt (x*x + y*y + z*z);
}

//==============================================================================
// ::GetRootLength ()
//==============================================================================
template <class T>
T G3::VEC2<T>::GetRootLength () const {
    return (x*x + y*y);
}

template <class T>
T G3::VEC3<T>::GetRootLength () const {
    return (x*x + y*y + z*z);
}

//==============================================================================
// ::GetL1Norm ()
//==============================================================================
template <class T>
T G3::VEC2<T>::GetL1Norm () const {
    return (G3_ABS (x) + G3_ABS (y));
}

template <class T>
T G3::VEC3<T>::GetL1Norm () const {
    return (G3_ABS (x) + G3_ABS (y) + G3_ABS (z));
}

//==============================================================================
// ::Normalize ()
//==============================================================================
template <class T>
void G3::VEC2<T>::Normalize () {
    double f = GetLength ();

	Assert (f, "VEC2X::Normalize: Normalizing a NULL vector");

    x = (T) (((double) x) / f);
    y = (T) (((double) y) / f);
}

template <class T>
void G3::VEC3<T>::Normalize () {
    double f = GetLength ();

	Assert (f, "VEC3X::Normalize: Normalizing a NULL vector");

    x = (T) (((double) x) / f);
    y = (T) (((double) y) / f);
	z = (T) (((double) z) / f);
}

//==============================================================================
// Swaps axis
//==============================================================================
template <class T>
void G3::VEC2<T>::SwapAxis () {
    T temp = x; x = y; y = temp;
}

//==============================================================================
// Calculates the angle of the vector's slope
//==============================================================================
template <class T>
VEC3<T> G3::VEC3<T>::GetSlopeAngle (double *Angle) {
    VEC3<T> a;
    double z_temp = z;

    if (z == 0.0)
        z_temp = G3_EPSILON;

    Assert (Angle, "VEC3X::GetSlopeAngle: Angle is a NULL pointer");

    *Angle = acos (((double) z_temp) / GetLength ());
    // The arc cosine function is an even function
    if (z_temp <= 0)
       *Angle = -(*Angle);

    return VEC3<T> ((T) (-y * z_temp), (T) (x * z_temp), 0);
}

//==============================================================================
// Gets a vector perpendicular to this one
//==============================================================================
template <class T>
VEC2<T> G3::VEC2<T>::GetPerpendicular () {
    return VEC2<T> (-y, x);
}

//==============================================================================
// The crossproduct of vectors
//==============================================================================
template <class T>
T G3::CrossProduct (const VEC2<T> &A, const VEC2<T> &B) {
      return ((A.x * B.y) - (B.x * A.y));
}

template <class T>
T G3::CrossProduct (const VEC2<T> &A, const VEC2<T> &B, const VEC2<T> &C) {
      return (B.x - A.x) * (C.y - A.y) - (C.x - A.x) * (B.y - A.y);
}

template <class T>
VEC3<T> G3::CrossProduct (const VEC3<T> &A, const VEC3<T> &B) {
      return VEC3<T> ( (A.y * B.z) - (A.z * B.y),
               			   (A.z * B.x) - (A.x * B.z),
               			   (A.x * B.y) - (A.y * B.x) );
}

//==============================================================================
// Finds angle between vectors
//==============================================================================
template <class T>
double G3::GetAngle (const VEC3<T> &A, const VEC3<T> &B) {
	VEC3<T> a = A;
	VEC3<T> b = B;

    a.Normalize ();
    b.Normalize ();

    return acos (a * b);
}

//==============================================================================
// Projects vector A onto vector B
//==============================================================================
template <class T>
VEC3<T> G3::ProjectVector (const VEC3<T> &A, const VEC3<T> &B) {
	return (B * ((A * B) / ((T) B.GetLength ())));
}

//==============================================================================
// Projects a point onto a line between LineA and LineB
//==============================================================================
template <class T>
VEC3<T> G3::ProjectPointOnLine (const VEC3<T> &Point, const VEC3<T> &LineA, const VEC3<T> &LineB) {
	return (Point + ProjectVector (Point - LineA, LineB - LineA));
}

//==============================================================================
// Calculates polygon normal
//==============================================================================
template <class T>
VEC3<T> G3::GetNormal (const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C) {
       VEC3<T> n = CrossProduct<T> (C - A, B - A);
       n.Normalize ();
       return n;
}

//==============================================================================
// Just finds the distance between 2 points
//==============================================================================
template <class T>
double G3::GetDistance (const VEC3<T> &A, const VEC3<T> &B) {
	return (B - A).GetLength ();
}

//==============================================================================
// Transforms point P into triangle ABC barycentric coordinates
//==============================================================================
template <class T>
VEC3<T> G3::Barycentric (const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C, const VEC3<T> &P) {
    VEC3<T> v, v0, v1, v2;

    v0 = B - A;
    v1 = C - A;
    v2 = P - A;

    // NOTE:: d00, d01, d11, d20, d21, denom are constant for a given triangle..
    T d00 = v0 * v0, d01 = v0 * v1, d11 = v1 * v1;
    T d20 = v2 * v0, d21 = v2 * v1;
    T denom = d00 * d11 - d01 * d01;

    // V-coordinate
    v.x = (d11 * d20 - d01 * d21) / denom;
    // W-coordinate
    v.z = (d00 * d21 - d01 * d20) / denom;
    // U-coordinate
    v.y = 1 - v.x - v.z;

    return v;
}

//==============================================================================
// Calculates the area of a 2D triangle
//==============================================================================
template <class T>
T G3::TriArea2 (const VEC2<T> &A, const VEC2<T> &B, const VEC2<T> &C) {
    return (A.x - B.x) * (B.y - C.y) - (B.x - C.x) * (A.y - B.y);
}

//==============================================================================
// Returns true if the quad is convex
//==============================================================================
template <class T>
bool G3::IsConvexQuad (const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C, const VEC3<T> &D) {
    VEC3<T> bda = CrossProduct (D - B, A - B);
    VEC3<T> bdc = CrossProduct (D - B, C - B);

    if (bda * bdc >= 0) return false;

    VEC3<T> acd = CrossProduct (C - A, D - A);
    VEC3<T> acb = CrossProduct (C - A, B - A);

    return (acd * acb < 0);
}

//==============================================================================
// Gets a random 2D unit vector
//==============================================================================
template <class T>
VEC2<T> G3::GetRandVector2 () {
	VEC2<T> v;

    // Pick a random Y between -1 and 1.
    v.y = GetRandMinMax<T> (-1, 1);
	// Get X from Pythagoras' pants
	v.x = (T) sqrt (1 - v.y * v.y);

	return v;
}

//==============================================================================
// Gets a random 3D unit vector
//==============================================================================
template <class T>
VEC3<T> G3::GetRandVector3 () {
	VEC3<T> v;

    // Pick a random Z between -1 and 1.
    v.z = GetRandMinMax<T> (-1, 1);

    // Get radius of this circle
    T r = (T) sqrt (1 - v.z * v.z);

    // Pick a random point on a circle.
    T t = GetRandMinMax<T> ((T) -G3_PI, (T) G3_PI);

    // Compute matching X and Y for our Z.
    v.x = (T) cos (t) * r;
    v.y = (T) sin (t) * r;

	return v;
}

//==============================================================================
// Casts a vector from one type to other
//==============================================================================
template <class T, class W>
VEC2<W> G3::CastVector2 (const VEC2<T> &v) {
	return VEC2<W> ((W)v.x, (W)v.y);
}

template <class T, class W>
VEC3<W> G3::CastVector3 (const VEC3<T> &v) {
	return VEC3<W> ((W)v.x, (W)v.y, (W)v.z);
}

//==============================================================================
// Explicit template instantiation
//==============================================================================
template class G3::VEC2<int>;
template class G3::VEC2<float>;
template class G3::VEC2<double>;

template class G3::VEC3<int>;
template class G3::VEC3<float>;
template class G3::VEC3<double>;

template int G3::CrossProduct (const VEC2<int> &A, const VEC2<int> &B);
template float G3::CrossProduct (const VEC2<float> &A, const VEC2<float> &B);
template double G3::CrossProduct (const VEC2<double> &A, const VEC2<double> &B);

template int G3::CrossProduct (const VEC2<int> &A, const VEC2<int> &B, const VEC2<int> &C);
template float G3::CrossProduct (const VEC2<float> &A, const VEC2<float> &B, const VEC2<float> &C);
template double G3::CrossProduct (const VEC2<double> &A, const VEC2<double> &B, const VEC2<double> &C);

template VEC3<int> G3::CrossProduct (const VEC3<int> &A, const VEC3<int> &B);
template VEC3<float> G3::CrossProduct (const VEC3<float> &A, const VEC3<float> &B);
template VEC3<double> G3::CrossProduct (const VEC3<double> &A, const VEC3<double> &B);

template double G3::GetAngle (const VEC3<int> &A, const VEC3<int> &B);
template double G3::GetAngle (const VEC3<float> &A, const VEC3<float> &B);
template double G3::GetAngle (const VEC3<double> &A, const VEC3<double> &B);

template VEC3<int> G3::ProjectVector (const VEC3<int> &A, const VEC3<int> &B);
template VEC3<float> G3::ProjectVector (const VEC3<float> &A, const VEC3<float> &B);
template VEC3<double> G3::ProjectVector (const VEC3<double> &A, const VEC3<double> &B);

template VEC3<int> G3::ProjectPointOnLine (const VEC3<int> &Point, const VEC3<int> &LineA, const VEC3<int> &LineB);
template VEC3<float> G3::ProjectPointOnLine (const VEC3<float> &Point, const VEC3<float> &LineA, const VEC3<float> &LineB);
template VEC3<double> G3::ProjectPointOnLine (const VEC3<double> &Point, const VEC3<double> &LineA, const VEC3<double> &LineB);

template VEC3<int> G3::GetNormal (const VEC3<int> &A, const VEC3<int> &B, const VEC3<int> &C);
template VEC3<float> G3::GetNormal (const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C);
template VEC3<double> G3::GetNormal (const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C);

template double G3::GetDistance (const VEC3<int> &A, const VEC3<int> &B);
template double G3::GetDistance (const VEC3<float> &A, const VEC3<float> &B);
template double G3::GetDistance (const VEC3<double> &A, const VEC3<double> &B);

template VEC3<int> G3::Barycentric (const VEC3<int> &A, const VEC3<int> &B, const VEC3<int> &C, const VEC3<int> &P);
template VEC3<float> G3::Barycentric (const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C, const VEC3<float> &P);
template VEC3<double> G3::Barycentric (const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C, const VEC3<double> &P);

template int G3::TriArea2 (const VEC2<int> &A, const VEC2<int> &B, const VEC2<int> &C);
template float G3::TriArea2 (const VEC2<float> &A, const VEC2<float> &B, const VEC2<float> &C);
template double G3::TriArea2 (const VEC2<double> &A, const VEC2<double> &B, const VEC2<double> &C);

template bool G3::IsConvexQuad (const VEC3<int> &A, const VEC3<int> &B, const VEC3<int> &C, const VEC3<int> &D);
template bool G3::IsConvexQuad (const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C, const VEC3<float> &D);
template bool G3::IsConvexQuad (const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C, const VEC3<double> &D);

template VEC2<int> G3::GetRandVector2 ();
template VEC2<float> G3::GetRandVector2 ();
template VEC2<double> G3::GetRandVector2 ();
template VEC3<int> G3::GetRandVector3 ();
template VEC3<float> G3::GetRandVector3 ();
template VEC3<double> G3::GetRandVector3 ();

template VEC2<int> G3::CastVector2 (const VEC2<float> &v);
template VEC2<int> G3::CastVector2 (const VEC2<double> &v);
template VEC2<float> G3::CastVector2 (const VEC2<int> &v);
template VEC2<float> G3::CastVector2 (const VEC2<double> &v);
template VEC2<double> G3::CastVector2 (const VEC2<int> &v);
template VEC2<double> G3::CastVector2 (const VEC2<float> &v);

template VEC3<int> G3::CastVector3 (const VEC3<float> &v);
template VEC3<int> G3::CastVector3 (const VEC3<double> &v);
template VEC3<float> G3::CastVector3 (const VEC3<int> &v);
template VEC3<float> G3::CastVector3 (const VEC3<double> &v);
template VEC3<double> G3::CastVector3 (const VEC3<int> &v);
template VEC3<double> G3::CastVector3 (const VEC3<float> &v);

